﻿#ifndef ROBOTCMDNODE_ROBOTSERVER_H_
#define ROBOTCMDNODE_ROBOTSERVER_H_

/*****************************************************************************
** Includes
*****************************************************************************/
// ROS2
#include "clawcmd_msg/msg/claw_cmd.hpp"
#include "jsonba_msg/msg/json_ba.hpp"
#include "maptext_msg/msg/map_text.hpp"
#include "param_msg/msg/param.hpp"
#include "pccmd_msg/msg/pc_cmd.hpp"
#include "rclcpp/rclcpp.hpp"
#include "serialtool_msg/msg/serial_tool.hpp"

// CPP
#include <chrono>
#include <functional>
#include <memory>
#include <string>

class RobotServerCmd;

/*****************************************************************************
 ** Class
 *****************************************************************************/

class RobotCmdNode : public rclcpp::Node {
public:
    RobotCmdNode(RobotServerCmd* cmdServer);
    virtual ~RobotCmdNode();

    void sendPcCmd(const std::string& cmdname, const std::string& cmdcontent);

    void clawControl(uint8_t claw_dir, uint8_t claw_force);

    void sendParamMsg(const std::string& cmdname,
                      const std::vector<std::string>& params);

private:
    /*********************
     ** Subscriber y
     ** Publisher
     **********************/

    rclcpp::Publisher<pccmd_msg::msg::PcCmd>::SharedPtr m_pubCmd;
    rclcpp::Publisher<clawcmd_msg::msg::ClawCmd>::SharedPtr m_pubClawCmd;
    rclcpp::Publisher<param_msg::msg::Param>::SharedPtr m_pubParam;

    rclcpp::Publisher<serialtool_msg::msg::SerialTool>::SharedPtr m_pubSerialTool;

    rclcpp::Subscription<jsonba_msg::msg::JsonBa>::SharedPtr m_subJsonBa;
    void jsonba_callback(const jsonba_msg::msg::JsonBa::SharedPtr msg) const;

    rclcpp::Subscription<maptext_msg::msg::MapText>::SharedPtr m_subMapText;
    void maptext_callback(const maptext_msg::msg::MapText::SharedPtr msg) const;

    RobotServerCmd* m_cmdServer;
};

#endif /* ROBOTCMDNODE_ROBOTSERVER_H_ */
